function P=MT_missile_step(G,P,k)
k2=opponent(k);
P2=G.P{k2};

P=set_field(P,'legend','');
ang0=atan2(P2.pos(2)-P.pos(2),P2.pos(1)-P.pos(1));
switch P.strategy
case 1 %%Tail Chasing (Pursuit Guidance)
    P.ang=ang0;
    P.pos=P.pos+P.speed*G.h*[cos(P.ang),sin(P.ang)];
case 2 %%Constant Bearing
    ang2=atan2(P2.pos(2)-P2.last_pos(2),P2.pos(1)-P2.last_pos(1));
    speed2=P2.speed;
    lead2=ang2-ang0;
    lead=asin(sin(lead2)*speed2/P.speed);
    P.ang=ang0+lead;
    P.pos=P.pos+P.speed*G.h*[cos(P.ang),sin(P.ang)];
case 3 %%Parallel Navigation (CATD)
    ang2=atan2(P2.pos(2)-P2.last_pos(2),P2.pos(1)-P2.last_pos(1));
    speed2=norm(P2.pos-P2.last_pos)/G.h;
    lead2=ang2-ang0;
    lead=asin(sin(lead2)*speed2/P.speed);
    if imag(lead)~=0, lead=0; end
    P.ang=ang0+lead;
    P.pos=P.pos+P.speed*G.h*[cos(P.ang),sin(P.ang)];
case 4 %%Beam Rider
    K=10;
    beam_pos=[0,0];
    pos_missile=P.pos-beam_pos;
    pos_target=P2.pos-beam_pos;
    ang_missile=atan2(pos_missile(2),pos_missile(1));
    ang_target=atan2(pos_target(2),pos_target(1));
    acc_missile=K*norm(pos_missile)*sin(ang_target-ang_missile);
    P.ang=P.ang+acc_missile/P.speed*G.h;
    P.pos=P.pos+P.speed*G.h*[cos(P.ang),sin(P.ang)];
case 5 %%Propotional Navigation Command
    K=P.PNG_constant;
    rx=P.pos(1)-P2.pos(1);
    ry=P.pos(2)-P2.pos(2);
    vx=P.speed*cos(P.ang)-P2.speed*cos(P2.ang);
    vy=P.speed*sin(P.ang)-P2.speed*sin(P2.ang);
    LOS_rate=(rx*vy-vx*ry)/(rx^2+ry^2);
    acc_missile=K*LOS_rate*P.speed;
    P.ang=P.ang+acc_missile/P.speed*G.h;
    P.pos=P.pos+P.speed*G.h*[cos(P.ang),sin(P.ang)];
case 6 %%Trajectory Learning
    %T_pos=P2.pos+rand(1,2)*P.measure_noise_magnitude;
    T_pos=P2.pos;
    P.T_trace=[P.T_trace;P2.pos];
    est_ang_speed=guess_ang_speed(P.T_trace,P.max_learn_step);
    P.T_ang_speed=est_ang_speed/G.h;
    T_traj=guess_target_trajectory(P.T_trace,P.T_ang_speed,P.max_predict_step);
    [est_timestep,est_miss_dis,best_ang]=guess_timestep_to_go(T_traj,P.pos,P.speed*G.h,G.min_r);
    msg=sprintf('Step %d: Angle Speed [%g] estimation %g , %d steps left, miss distance %g',G.step,P2.ang_speed,P.T_ang_speed,est_timestep,est_miss_dis);
    if mod(G.step,20)==0,disp(msg);end;
    P.ang=best_ang;
    P.pos=P.pos+P.speed*G.h*[cos(P.ang),sin(P.ang)];
end;

function [est_ang_speed,res]=guess_ang_speed(T,N)
dx=[ones(N+1,1);diff(T(:,1))];
dy=[ones(N+1,1);diff(T(:,2))];
cx=dx(end-N:end-1);
cy=dy(end-N:end-1);
bx=dx(end-N+1:end);
by=dy(end-N+1:end);
A1=[cx,-cy;cy,cx];
b1=[bx;by];
res=(A1'*A1)\(A1'*b1);
if res(2)>-1 & res(2)<1,
    est_ang_speed=asin(res(2));
else,
    est_ang_speed=0;
end;

function T_traj=guess_target_trajectory(T,est_ang_speed,L)
%T: past trajectory of target 
%est_ang_speed: estimated angle speed of target
%L: steps to predict 
dx=[zeros(1);diff(T(:,1))];
dy=[zeros(1);diff(T(:,2))];
cc=cos(est_ang_speed);
ss=sin(est_ang_speed);
rx=dx(end);
ry=dy(end);
px=T(end,1);
py=T(end,2);
T_traj=zeros(L,2);
for k=1:L;
    rx=rx*cc-ry*ss;
    ry=ry*cc+rx*ss;
    px=px+rx;
    py=py+ry;
    T_traj(k,:)=[px,py];
end;

function [est_timestep,est_miss_dis,best_ang]=guess_timestep_to_go(T_traj,M_pos,M_maxd,minr)
%T_traj: predicted target trajectory (step by step)
%M_pos: current missile position
%M_maxd: maximum distance that the missile can travel in one step
L=size(T_traj,1);
mis=zeros(L,1);
for k=1:L;
    mis(k)=norm(T_traj(k,:)-M_pos)-M_maxd*k;
end;
[ii]=find(abs(mis)<minr);
if isempty(ii),
    [dd,kk]=min(abs(mis));
else,
    kk=ii(1);
    dd=abs(mis(kk));
end;
est_timestep=kk;
est_miss_dis=dd;
rx=T_traj(kk,1)-M_pos(1);
ry=T_traj(kk,2)-M_pos(2);
best_ang=atan2(ry,rx);
